%求解控制序列为p、a_max时候的相关参数
%t_1 = x(3)、t_f = x(4)、c_1 = x(5)、c_2 = x(6)

%编写一个计算 F 的函数，即 x 处方程的值
    %中间变量t1时刻的速度与位移
F = @(x)[x(1)-12+0.5*x(5)-x(6)*x(3);
    x(2)-30+0.15*x(5)*x(3)*x(3)*x(3)-0.5*x(6)*x(3)*x(5)-30*x(3);
    %末态tf时刻的速度和位置
    x(1)+3*(x(4)-x(3))-12 * (1 - 0.15 * sin(0.2 * x(4)));
    x(2)+x(1)*(x(4)-x(3))+0.5*3*(x(4)-x(3))*(x(4)-x(3))+20+2*1.8*12*(1-0.15*sin(0.2*x(4)))-(100+12*x(4)+10*cos(0.2 * x(4)) - 10);
    %横截条件
    0.5 * 9 + 0.5 * 10 - (-x(5) * x(4) + x(6)) * 3 - (2 * 1.8 * x(5) + x(5) * x(4) - x(6)) * (-10 * cos(0.2 * x(4)));
    %协作变量
    -x(5)*x(3)+x(6)+4];

%创建初始点 x0 
x0 = [0;0;0;0;0;0];
%设置选项以返回迭代显示。
options = optimoptions('fsolve','Display','iter');